/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/helpInc/stop_bits.h
 * @Description  : 定义了NavStopBits类，用于管理和检查AGV的停止位信息。 
 *                 功能包含清空停止位、检查是否触发停止位和更新停止位信息的方法。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 17:04:09
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#pragma once
#include "rclcpp/rclcpp.hpp"
#include "tools.h"
#include <agv_msgs/msg/set_stop_bits.hpp>
#include <mutex>
#include <vector>
#include "timestamp.h"
#include <algorithm>
constexpr int STOP_BITS_TIME_OUT = 1000;
// 外部设置agv强制停止
class NavStopBits
{
public:
    NavStopBits() {
        Clear();
    }

    /**
     * @brief 清空停止位
     *
     * 该函数会锁定互斥锁，并清空 stop_bits_ 容器中的数据。
     */
    void Clear() {
        std::lock_guard<std::mutex> locker(mutex_);
        stop_bits_.clear();
    }

    /**
     * @brief 检查是否存在触发停止位
     *
     * 该函数用于检查是否存在触发停止位。它首先复制当前停止位集合，
     * 然后遍历这些停止位，对每个停止位进行有效性检查（未超时）和触发检查。
     * 如果存在至少一个有效的触发停止位，则返回true；否则返回false。
     *
     * @return 如果存在触发停止位，则返回true；否则返回false。
     */
    bool Occur() {
        // 暂不加读锁，防止影响主线程运行时序
        auto data_copy = stop_bits_;
        try {
            auto it = std::find_if(data_copy.begin(), data_copy.end(), [](STOP_BIT_TYPE& data) {
                // 如果数据超时，则无效
                if (data.TimeOut(STOP_BITS_TIME_OUT)) {
                    return false;
                }
                // 返回触发停止位
                return data.Get().stop > 0;
            });
            return it != data_copy.end();
        } catch (...) {
            DEBUG_ERROR_OUT("NavStopBits Occur crash");
            return false;
        }
        return false;
    }

    /**
     * @brief 更新停止位信息
     *
     * 该函数用于更新或添加停止位信息。如果已存在同名停止位信息，则更新该信息；否则，添加新的停止位信息。
     *
     * @param msg 包含停止位信息的消息对象
     */
    void Update(const agv_msgs::msg::SetStopBits& msg) {
        std::lock_guard<std::mutex> locker(mutex_);
        bool found = false;
        for (auto& data : stop_bits_) {
            if (data.Get().name == msg.name) {
                found = true;
                data.Update(msg);
                DEBUG_OUT("Update stopBits: stop=" << static_cast<int>(msg.stop) << ", name=" << msg.name);
                break;
            }
        }

        if (!found) {
            STOP_BIT_TYPE new_data;
            new_data.Update(msg);
            stop_bits_.push_back(new_data);
            DEBUG_OUT("Find new stopBits: stop=" << static_cast<int>(msg.stop) << ", name=" << msg.name);
        }
    }

private:
    using STOP_BIT_TYPE = DataTimeStamp<agv_msgs::msg::SetStopBits>;
    // 数据集
    std::vector<STOP_BIT_TYPE> stop_bits_;
    // 互斥锁
    std::mutex mutex_;
};
